#include <algorithm>
#include <Geex/basics/assert.h>
#include "packer.h"


namespace Field_packing
{
	Packer* Packer::instance_ = 0 ;
	
	Packer::Packer(const std::string& polygon_fn, const std::string& debug_fn, double coverage_rate) 
		: front_edge_sz(0.5), hole_face_sz(0.5), rot_allowed(true)
	{
		gx_assert(instance_ == 0) ;
		instance_ = this ;
		std::srand(std::time(0)/*0*/);
		rm_scale = 1.0;
		//rbox = new Rectangular_box(0.0, 0.0, 1.0, 1.0);
		std::string ext = Geex::FileSystem::extension(polygon_fn);
		read_polygon_set(polygon_fn, std::back_inserter(polygon_set), ext);			
		std::random_shuffle(polygon_set.begin(), polygon_set.end());

		double max_width = -std::numeric_limits<double>::max(), max_height = -std::numeric_limits<double>::max();
		total_volume = 0.0;
		for (unsigned int i = 0; i < polygon_set.size(); i++)
		{
			Packing_polygon::Iso_rectangle_2 bdbox = polygon_set[i]->bbox();
			max_width = std::max(bdbox.xmax() - bdbox.xmin(), max_width);
			max_height = std::max(bdbox.ymax() - bdbox.ymin(), max_height);
			if (polygon_set[i]->is_clockwise_oriented())
				polygon_set[i]->reverse_orientation();

			double vol = polygon_set[i]->volume();
			total_volume += vol;
		}

		// four square/triangle debug
		//max_width = 20;
		//rbox = new Rectangular_box(-10.0, -10.0, 10, 10);

		max_width *= 6.0;
		rbox = new Rectangular_box(0.0, 0.0, max_width, total_volume/0.7/max_width);		
		max_height = -std::numeric_limits<double>::max();
		
		Bin_packing_2 bp(*rbox);	
		for (unsigned int i = 0; i < polygon_set.size(); i++)
		{
			double x, y;
			CGAL::Iso_rectangle_2<K> bbox = polygon_set[i]->bbox();
			double w = bbox.xmax() - bbox.xmin(), h = bbox.ymax() - bbox.ymin();
			CGAL::Iso_rectangle_2<K> ebbox(bbox.xmin() - 0.05*w, bbox.ymin() - 0.05*h, bbox.xmax() + 0.05*w, bbox.ymax() + 0.05*h);
			if (bp.put_one_bin(ebbox, x, y))
				polygon_set[i]->translate(x, y);
			max_height = std::max(polygon_set[i]->bbox().ymax(), max_height);
		}
		rbox->lower(rbox->y_max() - 1.01*max_height);
		
		/* just for debug */
		//std::rotate(polygon_set.begin(), polygon_set.begin()+8, polygon_set.end());
		//std::random_shuffle(polygon_set.begin(), polygon_set.end());
		//size_t dbg_sz = 20;
		//for (unsigned int i = dbg_sz; i < polygon_set.size(); i++)
		//	delete polygon_set[i];
		//polygon_set.resize(dbg_sz);
		/************************/

		std::cout<<nb_polygons()<<" polygons are to be packed...\n";
		//std::cout<<"Constructing grid...\n";
		//dfg = new Distance_field_grid_2(*rbox, 20, 20);
		//for (size_t i = 0; i < polygon_set.size(); i++)
		//	dfg->append_distance_field(*polygon_set[i]);
		//spe = new Discrete_field_system(polygon_set, *rbox);
		sync_draw = false;
		gen_delaunay();
		//pdt.reset(new Delaunay_triangulation_2);
		//cat_cells.resize(nb_polygons());
		//db_cps = new Centralized_polygon_system(polygon_set, *rbox, rm_scale);
		nb_trials = 120;
		upward_gravity = false;
	}
	Packer::~Packer() 
	{
		delete rbox;
		for (std::vector<Packing_polygon*>::iterator it = polygon_set.begin(); it != polygon_set.end(); ++it)
			delete *it;
		polygon_set.clear();
		//delete spe;
		//delete dfg;
		//delete db_cps;
		instance_ = 0 ; 
	}


	void Packer::pack(unsigned int nb_trials)
	{

		static double opti_time = 0.0;
		static unsigned int opti_nb = 0;

		sync_draw = true;

//		CGAL::Timer timer;
		//System_potential_energy *spe = new Field_energy_system(polygon_set, *rbox);
		Discrete_field_system *spe = new Discrete_field_system(polygon_set, *rbox, trans_queue);
		double *trans = new double[nb_polygons()*3];
		//std::fill(trans, trans + nb_polygons()*3, 0.0);

		//timer.start();
		// BFGS
		int res = spe->optimize(trans, false);
		// steepest descent method
		//int res = spe->steepest_descent_optimize(trans);
		// directional descent
		//int res = spe->directional_descent_optimize(trans);
		if (res >= -400)
		{
			//std::cout<<"gradient at the solution is:\n";
			//double *gradient = new double[polygon_set.size()*3];
			//spe->energy_gradient_at(trans, gradient);
			//std::cout<<'(';
			//for (size_t i = 0; i < polygon_set.size(); i++)
			//	std::cout<<gradient[3*i]<<' '<<gradient[3*i+1]<<' '<<gradient[3*i+2]<<' ';
			//std::cout<<')'<<std::endl;
			//std::cout<<std::endl;
			//delete[] gradient;
			std::cout<<"Optimization finished."<<std::endl;
			while (!trans_queue.empty())
			{
				std::cout<<"Waiting for polygon display finish...\n";
			}
			//std::cout<<"Transform polygons...\n";
			for (unsigned int i = 0; i < nb_polygons(); i++)
			{
				polygon_set[i]->transform(Parameter(1.0, trans[i*3], trans[i*3+1], trans[i*3+2]));
				//std::cout<<"<"<<trans[3*i]<<','<<trans[3*i+1]<<','<<trans[3*i+2]<<">; ";
			}
			//std::cout<<std::endl;
		}
		else
		{
			std::cout<<"Minimization failed, error code = "<<res<<std::endl;
		}

		//timer.stop();
		//opti_time += timer.time();
		//opti_nb++;
		//std::cout<<"Average optimization time = "<<opti_time/opti_nb<<std::endl;
		//std::cout<<"Polygon position updated.\n";
		delete[] trans;
		delete spe;

		sync_draw = false;

		std::cout<<"Packing finished"<<std::endl;
		//glut_viewer_redraw();
	}

	double Packer::cen_pack()
	{
		typedef Packing_polygon::Transformation_2 Transformation_2;
		boost::shared_ptr<System_potential_energy> spe;
		double cur_energy;
		double *trans; // = new double[nb_polygons()*3];
		if (rot_allowed)
		{
			spe = boost::shared_ptr<System_potential_energy>(new Centralized_polygon_system(polygon_set, *rbox, rm_scale, upward_gravity));
			trans = new double[nb_polygons()*3];
		}
		else
		{
			spe = boost::shared_ptr<System_potential_energy>(new Centralized_polygon_system_no_rot(polygon_set, *rbox, rm_scale));
			trans = new double[nb_polygons()*2];
		}
		
		int res = spe->optimize(trans);
		if (res >= -400)
		{
			std::cout<<"Optimization finished(Error code: "<<res<<")."<<std::endl;
			//while (!trans_queue.empty())
			//{
			//	std::cout<<"Waiting for polygon display finish...\n";
			//}
			//std::cout<<"Transform polygons...\n";
			if (rot_allowed)
				for (unsigned int i = 0; i < nb_polygons(); i++)
				{		
					Packing_polygon::Point_2 c  = polygon_set[i]->centroid();
					Transformation_2 to_orig(CGAL::TRANSLATION, Packing_polygon::Vector(c, CGAL::ORIGIN));
					Transformation_2 t = Transformation_2(CGAL::TRANSLATION, Packing_polygon::Vector(trans[3*i+1], trans[3*i+2]))
										*Transformation_2(CGAL::ROTATION, std::sin(trans[i*3]), std::cos(trans[i*3]))*to_orig;
					polygon_set[i]->operator*=(t);
					pt.update_vertex_group(i, Transform_vertex(t));
					//std::cout<<"<"<<trans[3*i]<<','<<trans[3*i+1]<<','<<trans[3*i+2]<<">; ";
				}
			else
				for (unsigned int i = 0; i < nb_polygons(); i++)
				{
					Packing_polygon::Point_2 c  = polygon_set[i]->centroid();
					Transformation_2 to_orig(CGAL::TRANSLATION, Packing_polygon::Vector(c, CGAL::ORIGIN));
					Transformation_2 t = Transformation_2(CGAL::TRANSLATION, Packing_polygon::Vector(trans[2*i], trans[2*i+1]))*to_orig;
					polygon_set[i]->operator*=(t);
					pt.update_vertex_group(i, Transform_vertex(t));
					//std::cout<<"<"<<trans[3*i]<<','<<trans[3*i+1]<<','<<trans[3*i+2]<<">; ";
				}
			cur_energy = spe->system_energy_at(trans);
			//std::cout<<std::endl;
		}
		else
		{
			std::cout<<"Minimization failed, error code = "<<res<<std::endl;
			cur_energy = std::numeric_limits<double>::max();
		}
		delete[] trans;
		return cur_energy;
		//pt.update_triangulation();
	}
	
	void Packer::hand_select(double mouse_x, double mouse_y)
	{
		select_no = polygon_set.size();
		Packing_polygon::Point mouse_pnt(mouse_x, mouse_y);
		for (size_t i = 0; i < polygon_set.size(); i++)
		{
			if (!polygon_set[i]->has_on_unbounded_side(mouse_pnt))
			{
				select_no = i;
				break;
			}
		}
		std::cout<<"Selected "<<select_no<<std::endl;
	}

	double Packer::current_energy()
	{
		System_potential_energy *spe = new Centralized_polygon_system(polygon_set, *rbox, 1.0);
		std::cout<<"built system.\n";
		double *trans = new double[nb_polygons()*3];
		std::fill(trans, trans + nb_polygons()*3, 0.0);
		double e = spe->system_energy_at(trans);
		std::cout<<"system energy = "<<e<<std::endl;
		std::cout<<"gravity energy = "<<spe->gravity_energy_at(trans)<<std::endl;
		std::cout<<"molecular energy = "<<spe->molecular_energy_at(trans)<<std::endl;
		delete[] trans;
		delete spe;
		return e;
	}

	void Packer::lower_container()
	{
		double max_y = rbox->y_min(), min_y = rbox->y_max();
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			for (size_t j = 0; j < polygon_set[i]->size(); j++)
			{
				double y = polygon_set[i]->vertex(j).y();
				max_y = std::max(y, max_y);
				min_y = std::min(y, min_y);
			}
		}
		// pull down all the polygons vertically by min_y - rbox.y_min
		
		Transformation_2 pull_down(CGAL::TRANSLATION, Packing_polygon::Vector(0.0, rbox->y_min()-min_y));
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			polygon_set[i]->operator*=(pull_down);
			pt.update_vertex_group(i, Transform_vertex(pull_down));
		}
		double low_dist = 0.5*(rbox->y_max() - max_y + min_y - rbox->y_min());
		rbox->lower(low_dist);
		pt.update_vertex_group(-1, Push_top_sample_pnts(rbox->y_max()));
		pt.update_triangulation();
		std::cout<<"Area coverage = "<<total_volume/rbox->volume()<<std::endl;
	}

	void Packer::move(double dx, double dy)
	{
		//double box_width = rbox->x_max() - rbox->x_min();
		//double theta = boost::math::constants::pi<double>()/100;
		//Parameter p(1.0, theta, box_width/100, 0.0);
		if (select_no < polygon_set.size())
			polygon_set[select_no]->transform(Parameter(1.0, 0.0, dx, dy));
		//dfg->update_distance_field(polygon_id, p.to_transformation());
	}
	void Packer::get_bbox(double& x_min,double& y_min, double& z_min, double& x_max, double& y_max, double& z_max)
	{
		x_min = rbox->x_min(); x_max = rbox->x_max();
		y_min = rbox->y_min(); y_max = rbox->y_max();
		z_min = rbox->z_min(); z_max = rbox->z_max();
	}
	
	void Packer::gen_delaunay()
	{
		pt.begin_insert();
		for (size_t i = 0; i < polygon_set.size(); i++)
			pt.insert(*polygon_set[i], i);

		// sample on the container edges
		std::vector<Rectangular_box::Point> verts;
		rbox->vertices(verts);
		Packing_polygon container_poly(verts.begin(), verts.end());
		pt.insert(container_poly, -1);
		pt.end_insert();
	}
#if 1
	void Packer::extract_vacant_neighbor()
	{
		std::for_each(groups.begin(), groups.end(), std::mem_fun_ref(&std::list<int>::clear));
		groups.clear();
		std::for_each(vacant_space.begin(), vacant_space.end(), std::mem_fun_ref(&Hole::clear));
		if (vacant_space.size() == 0) vacant_space.resize(nb_polygons());
		for (size_t i = 0; i < nb_polygons(); i++)
			extract_vacant_neighbor(i, vacant_space[i]);

		const Triangulation_2& tri = pt.triangulation();
		// adjacency matrix
		std::vector< std::vector<bool> > adj_mat(nb_polygons());
		for (size_t i = 0; i < nb_polygons(); i++)
			adj_mat[i].resize(nb_polygons(), false);
		for (Triangulation_2::Edge_iterator eit = tri.edges_begin(); eit != tri.edges_end(); ++eit)
		{
			if (tri.is_infinite(*eit))	continue;
			Face_handle f = eit->first;
			int i = eit->second;
			Vertex_handle vs = f->vertex(f->ccw(i)), ve = f->vertex(f->cw(i));
			if (vs->group_id < 0 || ve->group_id < 0)	continue;
			if (vs->group_id != ve->group_id)
			{
				adj_mat[vs->group_id][ve->group_id] = adj_mat[ve->group_id][vs->group_id] = true;
			}
		}

		//std::list< std::list<int> > groups;
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			std::vector< std::list<int> >::iterator oit;
			for (oit = groups.begin(); oit != groups.end(); ++oit)
			{
				std::list<int>::iterator iit;
				for (iit = oit->begin(); iit != oit->end(); ++iit)
				{
					if (adj_mat[*iit][i])
						break;
				}
				if (iit == oit->end())
				{
					oit->push_back(i);
					break;
				}
			}
			if (oit == groups.end())
			{
				groups.push_back(std::list<int>());
				groups.back().push_back(i);
			}
		}
	}
#endif
	void Packer::extract_vacant_neighbor(size_t idx, Hole& vnbnd)
	{
		const Triangulation_2& tri = pt.triangulation();
		const std::list<Vertex_handle>& verts = pt.vertex_group(idx);
		for (std::list<Vertex_handle>::const_iterator it = verts.begin(); it != verts.end(); ++it)
		{
			Triangulation_2::Face_circulator nf = tri.incident_faces(*it), fs, fe;
			fs = fe = nf;
			do 
			{
				int i = fs->index(*it);
				Vertex_handle vs = fs->vertex(fs->ccw(i)), ve = fs->vertex(fs->cw(i));
				if (vs->group_id == ve->group_id)
					break;
				++fs;
			} while (fs != fe);
			fe = fs;
			do
			{
				int i = fs->index(*it);
				Vertex_handle vs = fs->vertex(fs->ccw(i)), ve = fs->vertex(fs->cw(i));
				if (vs->group_id != (*it)->group_id && ve->group_id != (*it)->group_id)
					vnbnd.segments.push_back(Segment_2(vs->point(), ve->point()));
				++fs;
			} while (fs != fe);
		}
		Point_2 c = CGAL::centroid(vnbnd.segments.begin(), vnbnd.segments.end(), CGAL::Dimension_tag<1>());
		vnbnd.area = 0.0;
		for (std::list<Segment_2>::const_iterator iit = vnbnd.segments.begin(); iit != vnbnd.segments.end(); ++iit)
			vnbnd.area += CGAL::area(c, iit->source(), iit->target());
		vnbnd.area = std::fabs(vnbnd.area);
	}

	void Packer::draw_vacant_space(size_t group_id)
	{
		glBegin(GL_LINES);
		for (std::list<int>::const_iterator oit = groups[group_id].begin(); oit != groups[group_id].end(); ++oit)
		{
			for (std::list<Segment_2>::const_iterator iit = vacant_space[*oit].segments.begin(); iit != vacant_space[*oit].segments.end(); ++iit)
			{
				glVertex2d(iit->source().x(), iit->source().y());
				glVertex2d(iit->target().x(), iit->target().y());
			}
		}
		glEnd();
	}

	void Packer::detect_bnd_holes()
	{
		const Triangulation_2& tri = pt.triangulation();
		holes.clear();
		// collect all the seed faces
		double avg_area = 0.0;
		for (size_t i = 0; i < polygon_set.size(); i++)
			avg_area += polygon_set[i]->area();
		avg_area /= polygon_set.size();
		double front_edge_len2_thrd = front_edge_sz*avg_area;
		double hole_face_area_thrd = hole_face_sz*avg_area;
		std::queue<Face_handle> seed_faces;
		for (Triangulation_2::Finite_faces_iterator fit = tri.finite_faces_begin(); fit != tri.finite_faces_end(); ++fit)
		{
			Vertex_handle v[3] = { fit->vertex(0), fit->vertex(1), fit->vertex(2) };
			if (v[0]->group_id == v[1]->group_id && v[1]->group_id == v[2]->group_id && v[2]->group_id == v[0]->group_id)
			{
				if (v[0]->group_id < 0)	
					fit->visited = false; 
				else
				{
					Point_2 c = CGAL::barycenter(v[0]->point(), 1.0/3, v[1]->point(), 1.0/3, v[2]->point(), 1.0/3);
					if (polygon_set[v[0]->group_id]->has_on_unbounded_side(c)) // if the triangle is outside the polygon
						fit->visited = false;
					else
						fit->visited = true;
				}
			}
			else
			{
				// all the triangles formed between different polygons considered as seeds
				fit->visited = false;
				double d01 = CGAL::squared_distance(v[0]->point(), v[1]->point()),
					d12 = CGAL::squared_distance(v[1]->point(), v[2]->point()),
					d20 = CGAL::squared_distance(v[2]->point(), v[0]->point());
				if (d01 >= front_edge_len2_thrd || d12 >= front_edge_len2_thrd || d20 >= front_edge_len2_thrd)
					seed_faces.push(fit);
			}
		}
		while (!seed_faces.empty())
		{
			Face_handle f = seed_faces.front();
			seed_faces.pop();
			if (f->visited)	continue;
			f->visited = true;
			std::list<Face_handle> ahole; // a hole consisting of triangle faces
			std::list<Edge> hole_bd;
			std::queue<Edge> active_front_edges;
			active_front_edges.push(Edge(f, 0));
			active_front_edges.push(Edge(f, 1));
			active_front_edges.push(Edge(f, 2));
			ahole.push_back(f);
			while (!active_front_edges.empty())
			{
				Edge cur_edge = active_front_edges.front();
				active_front_edges.pop();
				Face_handle fh = cur_edge.first;
				int vi = cur_edge.second;
				Vertex_handle vs = fh->vertex(fh->cw(vi)), vt = fh->vertex(fh->ccw(vi));
				double edge_len2 = CGAL::squared_distance(vs->point(), vt->point());
				if (edge_len2 >= front_edge_len2_thrd)
				{
					Face_handle neighbor_face = fh->neighbor(vi);
					if (!tri.is_infinite(neighbor_face) && !neighbor_face->visited)
					{
						int vsi = neighbor_face->index(vs), vti = neighbor_face->index(vt);
						active_front_edges.push(Edge(neighbor_face, vsi));
						active_front_edges.push(Edge(neighbor_face, vti));
						neighbor_face->visited = true;
						ahole.push_back(neighbor_face);
					}
					else
						hole_bd.push_back(cur_edge);
				}
				else
					hole_bd.push_back(cur_edge);
			}
			double hole_area = 0.0;
			for (std::list<Face_handle>::const_iterator it = ahole.begin(); it != ahole.end(); ++it)
			{
				hole_area += std::fabs(CGAL::area((*it)->vertex(0)->point(), (*it)->vertex(1)->point(), (*it)->vertex(2)->point()));
			}
			if (hole_area >= hole_face_area_thrd)
			{
				holes.push_back(Hole());
				for (std::list<Edge>::const_iterator it = hole_bd.begin(); it != hole_bd.end(); ++it)
				{
					Vertex_handle v0 = it->first->vertex(it->first->cw(it->second)), v1 = it->first->vertex(it->first->ccw(it->second));
					holes.back().segments.push_back(Segment_2(v0->point(), v1->point()));
				}
				holes.back().area = hole_area;
			}
		}
		std::cout<<holes.size()<<" holes were detected.\n";
		// sort the holes based on their area
		std::sort(holes.begin(), holes.end(), [](const Hole& lhs, const Hole& rhs)
												{ return (lhs.area > rhs.area); } );
	}
	bool Packer::overlap(const std::list<Segment_2>& hole, const Packing_polygon& pgn)
	{
		for (std::list<Segment_2>::const_iterator hit = hole.begin(); hit != hole.end(); ++hit)
		{
			for (Packing_polygon::Edge_const_iterator pit = pgn.edges_begin(); pit != pgn.edges_end(); ++pit)
			{
				if (CGAL::intersection(*hit, *pit))
					return true;
			}
		}
		return false;
	}
	void Packer::fill_holes()
	{
		// we only fill the several large holes with small tiles
		std::vector< std::pair<double, size_t> > area_idx;
		area_idx.reserve(polygon_set.size());
		for (size_t i = 0; i < polygon_set.size(); i++)
			area_idx.push_back(std::make_pair(polygon_set[i]->area(), i));
		std::sort(area_idx.begin(), area_idx.end(), [](const std::pair<double, size_t>& lhs, const std::pair<double, size_t>& rhs)
                                                    { return lhs.first > rhs.first; }	); 
		//std::for_each(area_idx.begin(), area_idx.end(), [](const std::pair<double, size_t>& item){ std::cout<<item.first<<": "<<item.second<<"; "; });											
		std::vector<bool> matched(polygon_set.size(), false);
		size_t n = std::min<size_t>(holes.size(), 5);
		size_t cnt = 0;
		for (size_t i = 0; i < n; i++)
		{
			Point_2 c = CGAL::centroid(holes[i].segments.begin(), holes[i].segments.end(), CGAL::Dimension_tag<0>());
			std::list<Segment_2> cent_hole;
			// centralized the hole
			Transformation_2 to_orig(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN));
			for (std::list<Segment_2>::const_iterator it = holes[i].segments.begin(); it != holes[i].segments.end(); ++it)
				cent_hole.push_back( it->transform(to_orig) );

			Polygon_matcher pm(cent_hole.begin(), cent_hole.end());
			for (size_t j = 0; j < area_idx.size(); j++)
			{
				if (area_idx[j].first >= holes[i].area || matched[area_idx[j].second])	continue;
				const Packing_polygon& pgn = *polygon_set[area_idx[j].second];
				Match_info_item<size_t> m = pm.affine_match(pgn, area_idx[j].second);
				//double s = std::sin(m.theta), c = std::cos(m.theta);
				//Packing_polygon::Transformation_2 t(c, -s, m.tx, s, c, m.ty);
				Packing_polygon tp = transform(m.t, pgn);
				// check containment and overlap
				if (!overlap(cent_hole, tp))
				{
					// move the polygon to fill the hole
					*polygon_set[area_idx[j].second] = transform(Transformation_2(CGAL::TRANSLATION, Vector_2(CGAL::ORIGIN, c)), tp);
					Transformation_2 tri_vtx_tran = Transformation_2(CGAL::TRANSLATION, Vector_2(CGAL::ORIGIN, c))*m.t;
					pt.update_vertex_group(area_idx[j].second, Transform_vertex(tri_vtx_tran));
					matched[area_idx[j].second] = true;
					cnt++;
					break;
				}
			}
		}
		pt.update_triangulation();
		std::cout<<cnt<<" hole"<<(cnt<=1?"":"s")<<" filled.\n";
	}
	void Packer::permute()
	{
		size_t nb_swap = 0;
		for (size_t i = 0; i < groups.size() && nb_swap < 1; i++)
			nb_swap = permute(groups[i]);

	}
	size_t Packer::permute(const std::list<int>& one_group)
	{
		std::function<Segment_2(const Transformation_2&, const Segment_2&)> tran_seg = [](const Transformation_2& t, const Segment_2& s){return Segment_2(t(s.source()), t(s.target()));};
		// similarity matrix
		std::vector< std::vector<double> > sim_mat(one_group.size());
		// measure similarity between vacant space and tiles
		size_t i = 0;
		std::vector< std::list<int>::const_iterator > idx_map(one_group.size());
		for (std::list<int>::const_iterator oit = one_group.begin(); oit != one_group.end(); ++oit, ++i) // iterate vacant space
		{
			idx_map[i] = oit;
			sim_mat[i].resize(one_group.size());
			const std::list<Segment_2>& segs = vacant_space[*oit].segments;
			Point_2 c = CGAL::centroid(segs.begin(), segs.end(), CGAL::Dimension_tag<1>());
			std::list<Segment_2> cent_hole;
			//std::transform(segs.begin(), segs.end(), std::back_inserter(cent_hole), std::bind1st(tran_seg, Transformation_2(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN))));
			for (std::list<Segment_2>::const_iterator it = segs.begin(); it != segs.end(); ++it)
				cent_hole.push_back(tran_seg(Transformation_2(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN)), *it));
			Polygon_matcher pm(cent_hole.begin(), cent_hole.end());
			size_t j = 0;
			for (std::list<int>::const_iterator iit = one_group.begin(); iit != one_group.end(); ++iit, ++j) // iteratie tiles
			{
				const Packing_polygon& pgn = *polygon_set[*iit];
				// for the current tile and hole pair, take the their hausdoff distance
				if (*oit == *iit)
				{
					std::vector<Point_2> hole_edge_pnts, tile_edge_pnts;
					edge_sample(segs.begin(), segs.end(), 50, hole_edge_pnts);
					edge_sample(pgn.edges_begin(), pgn.edges_end(), 50, tile_edge_pnts);
					std::valarray<double> dists(std::numeric_limits<double>::max(), tile_edge_pnts.size());
					for (size_t p = 0; p < tile_edge_pnts.size(); p++)
					{
						for (size_t q = 0; q < hole_edge_pnts.size(); q++)
						{
							double d = std::sqrt(CGAL::squared_distance(tile_edge_pnts[p], hole_edge_pnts[q]));
							dists[p] = std::min(dists[p], d); 
						}
					}
					sim_mat[i][j] = -dists.sum()/dists.size();
					continue;
				}
				double tile_area = pgn.area();
				if (tile_area >= vacant_space[*oit].area) // uncontainment
				{
					sim_mat[i][j] = -1e6 * std::sqrt(vacant_space[*oit].area); // assigned with a very small number
					//std::cout<<"overlap panelty = "<<sim_mat[i][j]<<std::endl;
				}
				else // possible containment
				{
					Match_info_item<int> match_res = pm.affine_match(*polygon_set[*iit], *iit);
					Packing_polygon tp = transform(match_res.t, pgn);
					if (overlap(cent_hole, tp))
					{
						sim_mat[i][j] = -1e6 * std::sqrt(vacant_space[*oit].area); // uncontainment, assigned with a small measure
						//std::cout<<"overlap panelty = "<<sim_mat[i][j]<<std::endl;
					}
					else
						sim_mat[i][j] = -match_res.error;
				}
			}
		}
		//std::cout<<"Matching finished.\n";
		//std::ofstream ofs("sim_mat.txt");
		//for (size_t r = 0; r < sim_mat.size(); r++)
		//{
		//	for (size_t c = 0; c < sim_mat[r].size(); c++)
		//	{
		//		ofs << sim_mat[r][c] << '\t';
		//	}
		//	ofs << std::endl;
		//}
		iAuction scheduler(sim_mat);
		scheduler.MainAlgo();
		// begin permutation
		size_t nb_swap = 0;
		std::unordered_map<size_t, Packing_polygon> swap_map;
		//ofs<<"schedule result:\n";
		for (size_t k = 0; k < idx_map.size(); k++)
		{
			size_t match_idx = scheduler.GetAssignedCol(k);
			size_t hole_idx = *idx_map[k], pgn_idx = *idx_map[match_idx];
			//ofs << '<' << k << ',' << match_idx << '>' << ' ';
			if (hole_idx == pgn_idx)	
				continue;
			// match again for finding the transformation
			const std::list<Segment_2>& segs = vacant_space[hole_idx].segments;
			Point_2 c = CGAL::centroid(segs.begin(), segs.end(), CGAL::Dimension_tag<1>());
			std::list<Segment_2> cent_hole;
			//std::transform(segs.begin(), segs.end(), std::back_inserter(cent_hole), bind1st(tran_seg, Transformation_2(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN))));
			for (std::list<Segment_2>::const_iterator it = segs.begin(); it != segs.end(); ++it)
				cent_hole.push_back(tran_seg(Transformation_2(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN)), *it));
			Polygon_matcher pm(cent_hole.begin(), cent_hole.end());
			Match_info_item<size_t> match_res = pm.affine_match(*polygon_set[pgn_idx], pgn_idx);
			Packing_polygon tp = transform(match_res.t, *polygon_set[pgn_idx]);
			//if (overlap(cent_hole, tp))
			//	std::cout<<"Error: overlap but matched!"<<std::endl;
			swap_map[hole_idx] = transform(Transformation_2(CGAL::TRANSLATION, Vector_2(CGAL::ORIGIN, c)), tp);
		}
		for (std::unordered_map<size_t, Packing_polygon>::const_iterator it = swap_map.begin(); it != swap_map.end(); ++it)
		{
			*polygon_set[it->first] = it->second;
			nb_swap++;
		}
		std::cout<<nb_swap<<" tile"<<(nb_swap<=1?" is":"s are")<<" swapped in this group.\n";
		return nb_swap;
	}
	
	void Packer::move_to_neighbor_state()
	{
		// find all pairs that can be possibly swapped
		std::vector< std::pair<size_t, size_t> > swappable_pairs;
		swappable_pairs.reserve(nb_polygons()*(nb_polygons()-1)/2);
		std::vector<double> polygon_area(nb_polygons());
		std::vector<double> cat_area(nb_polygons());
		std::transform(polygon_set.begin(), polygon_set.end(), polygon_area.begin(), [](const Packing_polygon* p){return std::fabs(p->area());});
		std::transform(pt.polygon_cat_cells().begin(), pt.polygon_cat_cells().end(), cat_area.begin(), [](const CGAL_polygon& p){return std::fabs(p.area());});
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			for (size_t j = i + 1; j < nb_polygons(); j++)
			{
				if (polygon_area[i] < cat_area[j] && polygon_area[j] < cat_area[i])
					swappable_pairs.push_back(std::make_pair(i, j));
			}
		}
		// to avoid starvation of some pairs, using random 
		std::srand(std::time(0));
		std::random_shuffle(swappable_pairs.begin(), swappable_pairs.end());
		std::vector<bool> swap_mark(nb_polygons(), false);
		const size_t nb_pair_to_swap = 5;
		size_t nb_pair_swapped = 0;
		for (std::vector< std::pair<size_t, size_t> >::const_iterator it = swappable_pairs.begin(); it != swappable_pairs.end() && nb_pair_swapped < nb_pair_to_swap; ++it)
		{
			size_t i = it->first, j = it->second;
			if (swap_mark[i] || swap_mark[j])
				continue;
			if (polygon_set[i]->lib_id() == polygon_set[j]->lib_id())
				continue;
	
			Polygon_matcher pm_ij(pt.polygon_cat_cells().at(i));
			Polygon_matcher pm_ji(pt.polygon_cat_cells().at(j));
			Match_info_item<size_t> mij = pm_ij.affine_match(*polygon_set[j], j);
			CGAL_polygon tpj = CGAL::transform(mij.t, *polygon_set[j]);
			if (!has_inclusion(pt.polygon_cat_cells().at(i), tpj))
				continue;
			Match_info_item<size_t> mji = pm_ji.affine_match(*polygon_set[i], i);
			CGAL_polygon tpi = CGAL::transform(mji.t, *polygon_set[i]);
			if (!has_inclusion(pt.polygon_cat_cells().at(j), tpi))
				continue;
			delete polygon_set[i];
			delete polygon_set[j];
			polygon_set[i] = new Packing_polygon(tpj.vertices_begin(), tpj.vertices_end());
			polygon_set[j] = new Packing_polygon(tpi.vertices_begin(), tpi.vertices_end());
			//pt.update_vertex_group(i, Transform_vertex(mji.t));
			//pt.update_vertex_group(j, Transform_vertex(mij.t));
			//pt.swap_vertex_group(i, j);
			swap_mark[i] = swap_mark[j] = true;
			nb_pair_swapped++;
		}
		//pt.update_triangulation();
		//gen_delaunay();
		std::cout<<nb_pair_swapped<<" pairs can be swapped.\n";
	}

	CGAL_polygon Packer::simplify_polygon(const CGAL_polygon& pgn)
	{
		typedef boost::geometry::model::d2::point_xy<double> point_2d;
		typedef boost::geometry::model::linestring<point_2d> linestring_2d;	
		//
		CGAL_polygon ang_sim_pgn;
		for (size_t i = 0; i < pgn.size(); i++)
		{
			size_t p = i - 1;
			if (i == 0)	p = pgn.size() - 1;
			size_t q = (i+1)%pgn.size();
			Vector_2 ip(pgn.vertex(i), pgn.vertex(p));
			Vector_2 iq(pgn.vertex(i), pgn.vertex(q));
			if (std::fabs(ip*iq/std::sqrt(ip.squared_length()*iq.squared_length())+1) > 1e-3)
				ang_sim_pgn.push_back(pgn.vertex(i));
		}		
		linestring_2d boost_pgn;
		double pm = 0.0;
		for (size_t i = 0; i < ang_sim_pgn.size(); i++)
		{
			Point_2 p = ang_sim_pgn.vertex(i);
			boost_pgn.push_back(boost::geometry::make<point_2d>(p.x(), p.y()));
			pm += std::sqrt(CGAL::squared_distance(p, ang_sim_pgn.vertex((i+1)%ang_sim_pgn.size())));
		}
		size_t sim_sz = ang_sim_pgn.size();
		linestring_2d sim_pgn;
		double divider = 1e15;
		while (sim_sz > 8)
		{
			sim_pgn.clear();
			boost::geometry::simplify(boost_pgn, sim_pgn, pm/divider);
			sim_sz = sim_pgn.size();
			divider /= 2;
		}

		std::cout<<"original # of verts: "<<pgn.size()<<std::endl;
		std::cout<<"simplified # of vertx: "<<sim_pgn.size()<<std::endl;
		std::vector<Point_2> verts(sim_pgn.size());
		std::transform(sim_pgn.begin(), sim_pgn.end(), verts.begin(), [](const point_2d& p){return Point_2(p.x(), p.y());});
		return CGAL_polygon(verts.begin(), verts.end());
		//return sim_pgn;
	}
	
	bool Packer::has_inclusion(const CGAL_polygon& out_pgn, const CGAL_polygon& in_pgn)
	{
		Point_2 c = CGAL::centroid(in_pgn.vertices_begin(), in_pgn.vertices_end(), CGAL::Dimension_tag<0>());
		Transformation_2 shrink(CGAL::SCALING, 0.99);
		Transformation_2 to_orig(CGAL::TRANSLATION, Vector_2(c, CGAL::ORIGIN));
		Transformation_2 to_cent(CGAL::TRANSLATION, Vector_2(CGAL::ORIGIN, c));
		CGAL_polygon shrunk_in_pgn = CGAL::transform(to_cent*shrink*to_orig, in_pgn);

		for (size_t i = 0; i < shrunk_in_pgn.size(); i++)
			if (out_pgn.has_on_unbounded_side(shrunk_in_pgn.vertex(i)))
				return false;		

		for (CGAL_polygon::Edge_const_iterator oit = out_pgn.edges_begin(); oit != out_pgn.edges_end(); ++oit)
		{
			for (CGAL_polygon::Edge_const_iterator iit = shrunk_in_pgn.edges_begin(); iit != shrunk_in_pgn.edges_end(); ++iit)
			{
				CGAL::Object o = CGAL::intersection(*oit, *iit);
				if (const Point_2 *ipnt = CGAL::object_cast<Point_2>(&o))
					return false;
			}
		}
	
		return true;	
	}
	void Packer::run_sa(void (*update_draw)())
	{
		std::srand(std::time(0));
		std::ofstream ofs("energy.txt");
		std::vector<Packing_polygon> cur_optimal(nb_polygons()); // optimal solution found up to now
		double min_energy = std::numeric_limits<double>::max();
		// get the initial temperature
		double t, t0;
		rm_scale = 2.0;
		Config_parameters::gravity_scale = 20.0;
		double cur_energy, pre_energy;
		cur_energy = cen_pack();
		if (update_draw)	update_draw();
		ofs<<cur_energy<<std::endl;
		rm_scale = 50.0;
		Config_parameters::gravity_scale = 1.0;
		cen_pack();
		if (update_draw)	update_draw();
		gen_delaunay();

		std::cout<<"Determining the initial temperature...\n";
		double se = 0.0;
		const size_t N = 10;
		for (size_t n = 0; n < 5; n++)
		{
			move_to_neighbor_state();
			pre_energy = cur_energy;
			rm_scale = 2.0;
			Config_parameters::gravity_scale = 20.0;
			cur_energy = cen_pack();
			if (update_draw)	update_draw();
			ofs<<cur_energy<<std::endl;
			se += (cur_energy - pre_energy);
			if (cur_energy < min_energy)
			{
				for (size_t j = 0; j < polygon_set.size(); j++)
					cur_optimal[j] = *polygon_set[j];
				min_energy = cur_energy;
			}
			rm_scale = 50.0;
			Config_parameters::gravity_scale = 1.0;
			cen_pack();
			if (update_draw)	update_draw();
			gen_delaunay();
		}

		t0 = se/5;
		t0 = -t0/std::log(0.8);
		t0 = std::fabs(t0);
		t = t0;	
		std::cout<<"t0 = "<<t0<<std::endl;
		std::cout<<"Start simulated annealing...\n";
		for (size_t i = 1; i <= N; i++)
		{
			std:cout<<">>>>>>>>>>>>> "<<i<<" <<<<<<<<<<<<<<\n";
			std::vector<Packing_polygon> prev_state;
			prev_state.reserve(nb_polygons());
			for (size_t j = 0; j < polygon_set.size(); j++)
				prev_state.push_back(*polygon_set[j]);

			rm_scale = 2;
			Config_parameters::gravity_scale = 20.0;
			pre_energy = cur_energy;
			cur_energy = cen_pack();
			if (update_draw)	update_draw();
			std::cout<<"newly computed energy: "<<cur_energy<<std::endl;
			if ( cur_energy < pre_energy )
			{
				//pre_energy = cur_energy;
				std::cout<<">> transfer to the new state because of low energy.\n";
			}
			else
			{
				double det = std::exp(-(cur_energy - pre_energy)/t);
				std::cout<<"exp{-delta/t}="<<det<<std::endl;
				double r = Geex::Numeric::random_float64();
				//if (det > 1.0)
				//	det = 0.0;
				if ( det < r ) // reject the new state
				{
					std::cout<<">> reject the new state.\n";
					for (size_t j = 0; j < nb_polygons(); j++)
						*polygon_set[j] = prev_state[j];
					cur_energy = pre_energy;
				}
				else
				{
					//pre_energy = cur_energy;
					std::cout<<">> transfer to the new state because of probability.\n";
				}
			}
			if (cur_energy < min_energy)
			{
				for (size_t j = 0; j < polygon_set.size(); j++)
					cur_optimal[j] = *polygon_set[j];
				min_energy = cur_energy;
			}
			t = t0*std::pow(1.0 - static_cast<double>(i)/N, 6);

			rm_scale = 50.0;
			Config_parameters::gravity_scale = 1.0;
			cen_pack();
			if (update_draw)	update_draw();
			gen_delaunay();
			ofs<<cur_energy<<std::endl;
			move_to_neighbor_state();
		}

		for (size_t i = 0; i < nb_polygons(); i++)
			*polygon_set[i] = cur_optimal[i];
	}
	void Packer::multistart_search(void (*update_draw)())
	{
		std::cout<<"Multistart searching start...\n";
		CGAL::Timer timer;
		timer.start();
		//std::srand(std::time(0));
		boost::mutex mt;
		//std::ofstream ofs("energy.txt");
		std::vector<Packing_polygon> cur_optimal(nb_polygons()); // optimal solution found up to now
		//double min_energy = std::numeric_limits<double>::max();
		rm_scale = 2.0;
		Config_parameters::gravity_scale = 20.0;
		double min_energy = cen_pack();
		if (update_draw)	update_draw();
		std::cout<<">> Initial locally optimal energy = "<<min_energy<<std::endl;

		rm_scale = 50.0;
		Config_parameters::gravity_scale = 1.0;
		//rbox->lower(-0.5*(rbox->y_max() - rbox->y_min()));
		cen_pack();
		gen_delaunay();
		if (update_draw)	update_draw();
		rm_scale = 2.0;
		Config_parameters::gravity_scale = 20.0;

		// begin multistart
		// determine the swappable pairs
		std::vector< std::pair<size_t, size_t> > swappable_pairs;
		//swappable_pairs.reserve(nb_polygons()*(nb_polygons()-1)/2);
		std::vector<double> polygon_area(nb_polygons());
		std::vector<double> cat_area(nb_polygons());
		std::transform(polygon_set.begin(), polygon_set.end(), polygon_area.begin(), [](const Packing_polygon* p){return std::fabs(p->area());});
		std::transform(pt.polygon_cat_cells().begin(), pt.polygon_cat_cells().end(), cat_area.begin(), [](const CGAL_polygon& p){return std::fabs(p.area());});
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			for (size_t j = i + 1; j < nb_polygons(); j++)
			{
				if (polygon_set[i]->lib_id() == polygon_set[j]->lib_id())
					continue;
				if (polygon_area[i] > cat_area[j] || polygon_area[j] > cat_area[i])
					continue;
				Polygon_matcher pm_ij(pt.polygon_cat_cells().at(i));
				Polygon_matcher pm_ji(pt.polygon_cat_cells().at(j));
				Match_info_item<size_t> mij = pm_ij.affine_match(*polygon_set[j], j);
				CGAL_polygon tpj = CGAL::transform(mij.t, *polygon_set[j]);
				if (!has_inclusion(pt.polygon_cat_cells().at(i), tpj))
					continue;
				Match_info_item<size_t> mji = pm_ji.affine_match(*polygon_set[i], i);
				CGAL_polygon tpi = CGAL::transform(mji.t, *polygon_set[i]);
				if (!has_inclusion(pt.polygon_cat_cells().at(j), tpi))
					continue;
				swappable_pairs.push_back(std::make_pair(i, j));
			}
		}
		std::cout<<">> number of swappable pairs = "<<swappable_pairs.size()<<std::endl;
		//const size_t nb_trials = 120;
		const size_t max_nb_swap_pair = 5;
#ifdef _CILK_
		cilk_for (size_t i = 0; i < nb_trials; i++)
#else
		for (size_t i = 0; i < nb_trials; i++)
#endif
		{
			std::vector<size_t> idx(swappable_pairs.size());
			for (size_t j = 0; j < idx.size(); j++)
				idx[j] = j;
			{
				boost::lock_guard<boost::mutex> lkg(mt);
				std::srand(std::time(0) + i);
				std::random_shuffle(idx.begin(), idx.end());
			}
			size_t nb_pair_swapped = 0;
			std::vector<bool> swap_mark(nb_polygons(), false);
			std::vector<Packing_polygon*> pset(nb_polygons());
			for (size_t j = 0; j < idx.size() && nb_pair_swapped < max_nb_swap_pair; j++)
			{
				//size_t i = it->first, j = it->second;
				size_t k = swappable_pairs[idx[j]].first, l = swappable_pairs[idx[j]].second;
				if (swap_mark[k] || swap_mark[l])
					continue;
				if (polygon_set[k]->lib_id() == polygon_set[l]->lib_id())
					continue;
	
				Polygon_matcher pm_kl(pt.polygon_cat_cells().at(k));
				Polygon_matcher pm_lk(pt.polygon_cat_cells().at(l));
				Match_info_item<size_t> mkl = pm_kl.affine_match(*polygon_set[l], l);
				CGAL_polygon tpl = CGAL::transform(mkl.t, *polygon_set[l]);
				if (!has_inclusion(pt.polygon_cat_cells().at(k), tpl))
					continue;
				Match_info_item<size_t> mlk = pm_lk.affine_match(*polygon_set[k], k);
				CGAL_polygon tpk = CGAL::transform(mlk.t, *polygon_set[k]);
				if (!has_inclusion(pt.polygon_cat_cells().at(l), tpk))
					continue;

				pset[k] = new Packing_polygon(tpl.vertices_begin(), tpl.vertices_end());
				pset[l] = new Packing_polygon(tpk.vertices_begin(), tpk.vertices_end());
				swap_mark[k] = swap_mark[l] = true;
				nb_pair_swapped++;
			} 
			for (size_t j = 0; j < nb_polygons(); j++)
			{
				if (!swap_mark[j])
				{
					pset[j] = new Packing_polygon(polygon_set[j]->vertices_begin(), polygon_set[j]->vertices_end());
				}
			}
			boost::shared_ptr<System_potential_energy> spe(new Centralized_polygon_system(pset, *rbox, rm_scale));
			double *trans = new double[nb_polygons()*3];
			int res = spe->optimize(trans);
			if (res >= -400)
			{
				double e = spe->system_energy_at(trans);
				std::cout<<"one locally optimal energy = "<<e<<std::endl;
				boost::lock_guard<boost::mutex> lkg(mt);
				if (e < min_energy)
				{
					min_energy = e;
					std::cout<<">> Computed locally optimal energy = "<<min_energy<<std::endl;
					for (unsigned int j = 0; j < nb_polygons(); j++)
					{		
						Packing_polygon::Point_2 c = pset[j]->centroid();
						Transformation_2 to_orig(CGAL::TRANSLATION, Packing_polygon::Vector(c, CGAL::ORIGIN));
						Transformation_2 t = Transformation_2(CGAL::TRANSLATION, Packing_polygon::Vector(trans[3*j+1], trans[3*j+2]))
											*Transformation_2(CGAL::ROTATION, std::sin(trans[j*3]), std::cos(trans[j*3]))*to_orig;
						pset[j]->operator*=(t);
						cur_optimal[j] = *pset[j];
					}
				}
			}
			delete[] trans;
			for (size_t j = 0; j < nb_polygons(); j++)
				delete pset[j];
		}
		for (size_t i = 0; i < nb_polygons(); i++)
		{
			*polygon_set[i] = cur_optimal[i];
		}
		timer.stop();
		rm_scale = 1.0;
		Config_parameters::gravity_scale = 20.0;
		std::cout<<"The optimal energy computed from multistart: "<<min_energy<<std::endl;
		std::cout<<"checked system energy: "<<current_energy()<<std::endl;
		std::cout<<"Multistart ends, time elapsed = "<<timer.time()/60<<" minutes\n";
	}
	void Packer::plot()
	{
		boost::shared_ptr<Discrete_field_system>spe(new Discrete_field_system(polygon_set, *rbox, trans_queue));
		spe->plot_graph();
	}
	void Packer::write_polygon(const char* fn, const CGAL_polygon& pgn) const
	{
		std::ofstream ofs(fn);
		size_t edge_idx = 0;
		for (size_t m = 0; m < pgn.size(); m++)
		{
			Point_2 v = pgn.vertex(m);
			ofs <<"v "<< v.x() << ' ' << v.y() << std::endl;
		}
		for (size_t m = 0; m < pgn.size(); m++, edge_idx++)
		{
			ofs << "e "<<edge_idx + 1<<' '<<(edge_idx+1)%pgn.size()+1<<std::endl;
		}
	}
	
}
